/*
 * scubuf.c, buffers for SmartCam skeleton system
 */
 
#define _XOPEN_SOURCE 500

#include <string.h>
#include <stdlib.h>
#include <signal.h>

#include <log.h>
#include <timer.h>
#include "scu.h"

//#define SCU_BUF_DEADLOCK_DETECTION
#define SCU_BUF_LOCKING

#if defined(DES) && !defined(SCU_BUF_LOCKING)
#define SCU_BUF_LOCKING
#endif

extern log_default_streams_t scul_buf;

scos_mutex_t scc_trans_mutex;
scos_cond_t scc_trans_condition;
table_t scc_trans_watches;

static int scu_buf_counter=0;
static int scu_buf_write_to_disk = 0;

int INTMIN(int a, int b)
{
  if (a < b)
    return a;
  else
    return b;
}

int scuBufferSetGlobalOption(int option, int value)
{
  switch (option)
  {
    case SCU_BUF_OPTION_WRITETODISK:
      scu_buf_write_to_disk = value;
      scu_buf_counter = scu_buf_write_to_disk;
      lprintf(STREAM(scul_buf.notice), "WRITETODISK option set to %d", value);
      break;
    default:
      lprintf(STREAM(scul_buf.warning), "Unknown global option '%s'", option);
      return -1;
  }

  return 0;
}

scu_buf_t scuBufferCreate(int size, uint_fast8_t maxreaders,
			  buf_destructor_t destructor)
{
  scu_buf_t buf;
  char filename[PATH_MAX];

  /* Lose one byte because of empty/full ambiguity */
  size = size + 1; 

  lprintf(STREAM(scul_buf.notice), "Creating new buffer of size %d, maxreaders %d",
          size, maxreaders);

#ifdef SCU_BUF_CANARY
  buf.canary = SCU_BUF_CANARY;
#endif

  abortOnNULL(STREAM(scul_buf.err),
              buf.reader = malloc(maxreaders * sizeof(scu_bufptr_t)));
  buf.readers = 0;
  buf.maxreaders = maxreaders;
  buf.writer.ptr = buf.writer.newptr = 0;
  scos_mutex_init(&buf.lock);
  
#ifdef SCU_BUF_LOCKING
  scos_mutex_init(&buf.writelock);
  scos_mutex_init(&buf.readlock);
  scos_cond_init(&buf.writecondition);
  scos_cond_init(&buf.readcondition);
#endif

  buf.locked = 1;
  abortOnNULL(STREAM(scul_buf.err), buf.data = calloc(size+size, 1 /* *** TODO: CHANGE TO unit *** */));
  buf.size = size;
  buf.total = 0;
  buf.finalized = 0;
  buf.destructor = destructor;
  buf.timer = tmrInit();
  buf.waits = 0;

  buf.counter = scu_buf_counter++;

  if (scu_buf_write_to_disk)
  {
    sprintf(filename, "%s/stream%d.scs", SCU_BUF_PATH, buf.counter);
    buf.file = fopen(filename, "w");
  }
  else
    buf.file = NULL;

  buf.references = 1; /* Writer */

  return buf;  
}

int scuBufferSetIndex(scu_buf_t *buf, int index)
{
  buf->index = index; 
  return 0;
}

int scuBufferDestroy(scu_buf_t *buf)
{
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.notice), "Destroying buffer %p (%d waits)", buf, buf->waits);

  if (buf->references || buf->locked)
  {
    lprintf(STREAM(scul_buf.err), "Tried to destroy active buffer");
    return -1;
  }

  if (buf->file) fclose(buf->file);

  free(buf->data);
  free(buf->reader);

  scos_mutex_unlock(&buf->lock);
  scos_mutex_destroy(&buf->lock);

  scos_mutex_destroy(&buf->readlock);
  scos_mutex_destroy(&buf->writelock);
  scos_cond_destroy(&buf->writecondition);
  scos_cond_destroy(&buf->readcondition);
  
#ifdef SCU_BUF_CANARY
  buf->canary = ~SCU_BUF_CANARY;
#endif  

  if (buf->destructor) buf->destructor(buf);

  return 0;
}

int scuBufferRemoveReference(scu_buf_t *buf)
{
  int freed = 0;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  tmrStop(&buf->timer);
  lprintf(STREAM(scul_buf.info), "Removing buffer reference (now %d, %5.2f Mbps)",
	  buf->references-1, (double)buf->total/tmrGetValueInUs(&buf->timer)*8.0);

  scos_mutex_lock(&buf->lock);

  buf->references--;

  if (!buf->references)
  {
    if (!buf->locked)
    {
      lprintf(STREAM(scul_buf.notice), "No more references, destroying buffer");

      scuBufferDestroy(buf);
      freed = 1;
    }
    else
    {
      lprintf(STREAM(scul_buf.info), "Buffer locked, keeping it around");
      scos_mutex_unlock(&buf->lock);
    }
  }
  else
    scos_mutex_unlock(&buf->lock);

  return freed;
}

int scuBufferFinalize(scu_buf_t *buf)
{
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.notice), "Finalizing buffer (%d bytes served)",
	  buf->total);

#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->readlock);
#endif
  buf->finalized = 1;
#ifdef SCU_BUF_LOCKING
  scos_cond_signal(&buf->readcondition);
  scos_mutex_unlock(&buf->readlock);
#endif

  return scuBufferRemoveReference(buf);
}

int scuBufferIsDry(scu_buf_t *buf, scu_bufptrid_t ptr)
{
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  return (buf->finalized && (buf->reader[ptr].ptr == buf->writer.ptr));
}

scu_bufptrid_t scuBufferAddReader(scu_buf_t *buf)
{
  int reader;
  
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  if (!buf->locked)
  {
    lprintf(STREAM(scul_buf.err), "Attempt to add reader to unlocked buffer");
    return -1;
  }
  
  scos_mutex_lock(&buf->lock);
  
  if (buf->readers == buf->maxreaders)
  {
    lprintf(STREAM(scul_buf.err), "Too many readers");
    scos_mutex_unlock(&buf->lock);
    return -1;
  }
  
  lprintf(STREAM(scul_buf.notice), "Adding reader");

  /* Initialize new reader */
  reader = buf->readers;
  buf->reader[reader].ptr = buf->reader[reader].newptr = 0;
  
  /* Increase number of readers */
  buf->readers++;
  buf->references++;
  
  scos_mutex_unlock(&buf->lock);
  
  return reader;
}

int scuBufferUnlock(scu_buf_t *buf)
{
  int freed = 0;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  scos_mutex_lock(&buf->lock);

  if (!buf->locked)
  {
    lprintf(STREAM(scul_buf.err), "Attempt to unlock already unlocked buffer");
    scos_mutex_unlock(&buf->lock);
    return -1;
  }
 
  lprintf(STREAM(scul_buf.notice), "Unlocking buffer");
 
#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->writelock);
#endif
  buf->locked = 0;
#ifdef SCU_BUF_LOCKING
  scos_cond_signal(&buf->writecondition);
  scos_mutex_unlock(&buf->writelock);
#endif

  if (!buf->references)
  {
    lprintf(STREAM(scul_buf.notice), "No references, destroying buffer");
    scuBufferDestroy(buf);
    freed = 1;
  }
  else
    scos_mutex_unlock(&buf->lock);

  return freed;
}

int
scuBufferSpaceAvailable(scu_buf_t *buf)
{
  int avail, i, diff;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  if (buf->locked)
  {
    /* Locked buffer cannot wrap around */
    avail = buf->size - buf->writer.ptr - 1;
  }
  else  
  {
    /* Look for laziest reader */
    avail = buf->size;
    for (i = 0; i < buf->readers; i++)
    {
      diff = buf->reader[i].ptr - buf->writer.ptr;
      if (diff <= 0) diff = diff + buf->size;
      
      avail = MIN(avail, diff);
    }

    /* Lose one byte because of empty/full ambiguity */
    avail--;
  }

  return avail;
}

int
scuBufferWrite(scu_buf_t *buf,
               void *data,
               int size,
               int options)
{
  int i;
  int copy, copyback, copyfront, copied=0, oldptr, newptr;
  
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  if (size < 0)
  {
    lprintf(STREAM(scul_buf.err), "Trying to write negative amount of bytes (%d)",
	    size);
    return -1;
  }

  lprintf(STREAM(scul_buf.debug), "Going to write %d bytes to buffer %p (opt %d)",
	  size, buf, options);
  
  do
  {    
    /* Determine the amount of data we can write this pass */
    copy = INTMIN(scuBufferSpaceAvailable(buf), (int)size-copied);

    lprintf(STREAM(scul_buf.crawl), "Going to write %d bytes this pass", copy);
    
    /* Write data in two passes -- after and before writer.ptr */
    copyback = MIN(copy, buf->size - buf->writer.ptr);
    copyfront = copy-copyback;
    if (copyback) memcpy((char*)buf->data+buf->writer.ptr, (char*)data+copied, copyback);
    if (copyfront) memcpy((char*)buf->data, (char*)data+copied+copyback, copyfront); 
  
    /* Move writepointer */
    oldptr = buf->writer.ptr;
    newptr = buf->writer.ptr + copy;
    if (newptr >= buf->size) newptr -= buf->size;

#ifdef SCU_BUF_LOCKING
    scos_mutex_lock(&buf->readlock);
#endif
    buf->writer.ptr = newptr;
#ifdef SCU_BUF_LOCKING
    scos_cond_signal(&buf->readcondition);
    scos_mutex_unlock(&buf->readlock);
#endif

    /* Move peekpointer if we're writing more than we've peeked */
    if (oldptr < newptr)
    {
      if (buf->writer.newptr >= oldptr && buf->writer.newptr < newptr)
        buf->writer.newptr = newptr;
    }
    else
    {
      if (buf->writer.newptr >= oldptr || buf->writer.newptr < newptr)
        buf->writer.newptr = newptr;
    }
    
    if (!buf->total) tmrStart(&buf->timer);
    buf->total += copy;
    if (buf->file) fwrite((char*)data+copied, 1, copy, buf->file);
 
    copied += copy;
    
    if (copied < size && (options & SCU_BUF_BLOCK_ALL))
    {      
      lprintf(STREAM(scul_buf.crawl), "%d bytes written, blocking before next write pass", copied);

      newptr = newptr + 1;
      if (newptr >= buf->size) newptr -= buf->size;

      /* Block until everyone reads something */
      i = -1;

#ifdef SCU_BUF_LOCKING
      scos_mutex_lock(&buf->writelock);
#endif
      while (i != buf->readers)
      {
        if (buf->locked)
        {
          lprintf(STREAM(scul_buf.crawl), "Waiting for buffer to be unlocked...");
        }
        else
        {
          lprintf(STREAM(scul_buf.crawl), "Checking available space");
          for (i = 0; i < buf->readers; i++)
            if (buf->reader[i].ptr == newptr)
              break;
        }
        if (i != buf->readers)
#ifdef SCU_BUF_LOCKING
          scos_cond_wait(&buf->writecondition, &buf->writelock);
          buf->waits++;
#else
          scos_yield();
#endif
      }
#ifdef SCU_BUF_LOCKING
      scos_mutex_unlock(&buf->writelock);
#endif

    }
  } while (copied < size && (options & SCU_BUF_BLOCK_ALL));

  lprintf(STREAM(scul_buf.debug), "Wrote %d bytes", copied);

  return copied;  
}

int
scuBufferDataAvailable(scu_buf_t *buf,
		       scu_bufptrid_t r)
{
  int avail;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  avail = buf->writer.ptr - buf->reader[r].ptr;
  if (avail < 0) avail = avail + buf->size;
  
  return avail;
}

int
scuBufferRead(scu_buf_t *buf,
              scu_bufptrid_t r,
              void *data,
              int size,
              int options)
{
  int copy, copyfront, copyback, copied=0, oldptr, newptr;
  
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.debug),
	  "Going to read %d bytes from buffer %p (opt %d)", size, buf, options);

  do
  {
    /* Determine the amount of data we can read this pass */
    copy = INTMIN(scuBufferDataAvailable(buf, r), size-copied);

    lprintf(STREAM(scul_buf.crawl), "Going to read %d bytes this pass", copy);

    /* Read data in two passes -- after and before reader[r].ptr */
    copyback = MIN(copy, buf->size - buf->reader[r].ptr);
    copyfront = copy-copyback;
    if (copyback) memcpy((char*)data+copied, (char*)buf->data+buf->reader[r].ptr, copyback);
    if (copyfront) memcpy((char*)data+copied+copyback, buf->data, copyfront); 
    
    /* Move readpointer */
    oldptr = buf->reader[r].ptr;
    newptr = buf->reader[r].ptr + copy;
    if (newptr >= buf->size) newptr -= buf->size;

#ifdef SCU_BUF_LOCKING
    scos_mutex_lock(&buf->writelock);
#endif
    buf->reader[r].ptr = newptr;
#ifdef SCU_BUF_LOCKING
    scos_cond_signal(&buf->writecondition);
    scos_mutex_unlock(&buf->writelock);
#endif
#ifdef DES
    scos_mutex_lock(&scc_trans_mutex);
    if (tblget(&scc_trans_watches, buf->index, NULL))
    {
      //lprintf(STREAM(scul_buf.notice), "Signalling watch for buffer %d", buf->index);
      abortOnNegative(STREAM(scul_buf.err), tblswitchslice(&scc_trans_watches, 1, buf->index, SCU_TABLE_MEMBER));
      scos_cond_signal(&scc_trans_condition);
    }
    scos_mutex_unlock(&scc_trans_mutex);
#endif

    /* Move peekpointer if we're reading more than we've peeked */
    if (oldptr < newptr)
    {
      if (buf->reader[r].newptr >= oldptr && buf->reader[r].newptr < newptr)
        buf->reader[r].newptr = newptr;
    }
    else
    {
      if (buf->reader[r].newptr >= oldptr || buf->reader[r].newptr < newptr)
        buf->reader[r].newptr = newptr;
    }

    oldptr = oldptr - 1;
    if (oldptr < 0) oldptr = oldptr + buf->size;

    copied += copy;
    
    if ((copied < size && (options & SCU_BUF_BLOCK_ALL)) ||
   	    (copied == 0 && (options & SCU_BUF_BLOCK_ANY)))
    {
      lprintf(STREAM(scul_buf.crawl), "%d bytes read, blocking before next read pass", copied);

      /* Block until writer writes something */
      
#ifdef SCU_BUF_LOCKING
      scos_mutex_lock(&buf->readlock);
#endif
      while (buf->writer.ptr == newptr && !buf->finalized)
      {
        lprintf(STREAM(scul_buf.crawl), "Checking available data");
#ifdef SCU_BUF_LOCKING
        scos_cond_wait(&buf->readcondition, &buf->readlock);      
        buf->waits++;
#else
        scos_yield();
#endif
      }
#ifdef SCU_BUF_LOCKING
      scos_mutex_unlock(&buf->readlock);
#endif
    }
  } while (((copied < size && (options & SCU_BUF_BLOCK_ALL)) ||
	    (copied == 0 && (options & SCU_BUF_BLOCK_ANY))) &&
           !scuBufferIsDry(buf, r));

  lprintf(STREAM(scul_buf.debug), "Read %d bytes", copied);

  return copied;
}

/* ******************************************************************* */

inline int
scuBufferPreSpaceAvailable(scu_buf_t *buf)
{
  int avail, i, diff;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  if (buf->locked)
  {
    /* Locked buffer cannot wrap around */
    avail = buf->size - buf->writer.newptr - 1;
  }
  else  
  {
    /* Look for laziest reader */
    avail = buf->size;
    for (i = 0; i < buf->readers; i++)
    {
      diff = buf->reader[i].ptr - buf->writer.newptr;
      diff += (diff<=0)*buf->size;
      
      avail = MIN(avail, diff);

      //lprintf(STREAM(scul_buf.crawl), "reader %d: %d -> %d", i, diff-1, avail-1);
    }
    
    /* Lose one byte because of empty/full ambiguity */
    avail--;
  }

  return avail;
}

int
scuBufferAllocate(scu_buf_t *buf,
                  void **data,
                  int size,
                  int options)
{
  int avail;
#ifdef SCU_BUF_DEADLOCK_DETECTION
  tmr_t timer = tmrInit();
#endif
  
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.debug), "Going to allocate %d bytes in buffer %p (opt %d)",
	  size, buf, options);
  
#ifdef SCU_BUF_DEADLOCK_DETECTION
  tmrStart(&timer);
#endif

#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->writelock);
#endif
  while ((avail=scuBufferPreSpaceAvailable(buf)) < ((options & SCU_BUF_BLOCK_ALL)?size:1))
  {
    lprintf(STREAM(scul_buf.crawl), "%d bytes available, blocking before next allocate pass", avail);
#ifdef SCU_BUF_LOCKING
    scos_cond_wait(&buf->writecondition, &buf->writelock);
    buf->waits++;
#else
#ifdef SCU_BUF_DEADLOCK_DETECTION
    tmrStop(&timer);
    if (tmrGetValueInUs(&timer) > 10000000L)
    {
      lprintf(STREAM(scul_buf.err), "Deadlock trying to allocate %d bytes in buffer (opt %d). %s, %ld bytes served",
                  size, options, buf->locked?"Locked":"Not locked", buf->total);
                  
      buf->destructor(buf);
                  
      while (1) scos_yield();
    }
#endif
    scos_yield();
#endif
  } 
#ifdef SCU_BUF_LOCKING
  scos_mutex_unlock(&buf->writelock); 
#endif

  size = MIN(size, avail);

  *data = &buf->data[buf->writer.newptr];

  /* Move peekpointer */
  buf->writer.newptr = buf->writer.newptr + size;
  if (buf->writer.newptr >= buf->size) buf->writer.newptr -= buf->size;

  return size;
}

/* *** WARING: NO CHECKING! *** */
void scuBufferReleaseAllocated(scu_buf_t *buf, int size)
{
  int newptr;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.debug), "Releasing %d allocated bytes in buffer",
	  size);

  newptr = buf->writer.ptr + size;
  if (newptr >= buf->size)
  {
    lprintf(STREAM(scul_buf.crawl), "Copying %d bytes from end of buffer to start",
  	  newptr-buf->size);

    memcpy(buf->data, &buf->data[buf->size], newptr-buf->size);
    newptr -= buf->size;
  }

  if (buf->file) fwrite(&buf->data[buf->writer.ptr], 1, size, buf->file);
  if (!buf->total) tmrStart(&buf->timer);
  buf->total += size;

#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->readlock);
#endif
  buf->writer.ptr = newptr;
#ifdef SCU_BUF_LOCKING
  scos_cond_signal(&buf->readcondition);
  scos_mutex_unlock(&buf->readlock);
#endif

  //lprintf(STREAM(scul_buf.crawl), "Signalled");
}

inline int
scuBufferPreDataAvailable(scu_buf_t *buf,
		       scu_bufptrid_t r)
{
  int avail;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  avail = buf->writer.ptr - buf->reader[r].newptr;
  if (avail < 0) avail = avail + buf->size;

  return avail;
}

int
scuBufferPeek(scu_buf_t *buf,
              scu_bufptrid_t r,
              void **data,
              int size,
              int options)
{
  int avail;
#ifdef SCU_BUF_DEADLOCK_DETECTION
  tmr_t timer;
#endif
  
#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.debug), "Going to peek %d bytes in buffer %p (opt %d)",
	  size, buf, options);
  
#ifdef SCU_BUF_DEADLOCK_DETECTION
  tmrStart(&timer);
#endif

#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->readlock);
#endif
  while ((avail=scuBufferPreDataAvailable(buf, r)) < ((options & SCU_BUF_BLOCK_ALL)?size:1) && !buf->finalized)
  {
    lprintf(STREAM(scul_buf.crawl), "%d bytes available, blocking before next peek pass", avail);
#ifdef SCU_BUF_LOCKING
    scos_cond_wait(&buf->readcondition, &buf->readlock);
    buf->waits++;
#else
#ifdef SCU_BUF_DEADLOCK_DETECTION
    tmrStop(&timer);
    if (tmrGetValueInUs(&timer) > 10000000L)
    {
      lprintf(STREAM(scul_buf.err), "Deadlock trying to peek %d bytes in buffer (opt %d). %s, %ld bytes served",
              size, options, buf->locked?"Locked":"Not locked", buf->total);
      while (1) scos_yield();
    }
#endif
    scos_yield();
#endif
  }  
#ifdef SCU_BUF_LOCKING
  scos_mutex_unlock(&buf->readlock);
#endif

  size = MIN(size, avail);

  *data = &buf->data[buf->reader[r].newptr];

  /* Move peekpointer */
  buf->reader[r].newptr = buf->reader[r].newptr + size;
  if (buf->reader[r].newptr >= buf->size)
  {
    lprintf(STREAM(scul_buf.crawl), "Copying %d bytes from start of buffer to end",
  	  buf->reader[r].newptr-buf->size);
    memcpy(&buf->data[buf->size], buf->data, buf->reader[r].newptr-buf->size);
    buf->reader[r].newptr -= buf->size;
  }

  return size;
}

/* *** WARING: NO CHECKING! *** */
void scuBufferReleasePeeked(scu_buf_t *buf, scu_bufptrid_t r, int size)
{
  int newptr;

#ifdef SCU_BUF_CANARY
  SCU_BUF_CHECK_CANARY(buf);
#endif

  lprintf(STREAM(scul_buf.debug), "Releasing %d peeked bytes in buffer",
	  size);

  newptr = buf->reader[r].ptr + size;
  if (newptr >= buf->size) newptr -= buf->size;

#ifdef SCU_BUF_LOCKING
  scos_mutex_lock(&buf->writelock);
#endif
  buf->reader[r].ptr = newptr;
#ifdef SCU_BUF_LOCKING
  scos_cond_signal(&buf->writecondition);
  scos_mutex_unlock(&buf->writelock);
#endif
#ifdef DES
  scos_mutex_lock(&scc_trans_mutex);
  if (tblget(&scc_trans_watches, buf->index, NULL))
  {
    //lprintf(STREAM(scul_buf.notice), "Signalling watch for buffer %d", buf->index);
    abortOnNegative(STREAM(scul_buf.err), tblswitchslice(&scc_trans_watches, 1, buf->index, SCU_TABLE_MEMBER));
    scos_cond_signal(&scc_trans_condition);
  }
  scos_mutex_unlock(&scc_trans_mutex);
#endif
}
